#include <cv_bridge/cv_bridge.h>
#include <glog/logging.h>
#include <image_transport/image_transport.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <signal.h>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <thread>
#include "commons/rabbit_consts.h"
#include "commons/x264_encoder.h"
#include "commons/libar8020.h"

using namespace cv;
std::mutex front_mutex;
std::mutex back_mutex;
cv::Mat framefront;
cv::Mat frameback;
void MySigintHandler(int sig) {
  ROS_INFO("shutting down!");
  ros::shutdown();
  exit(0);
}
void videoFuse(Mat &big,Mat &small)
{
   Rect roi = Rect(0,0, small.cols, small.rows);
   Mat subView = big(roi); 
   small.copyTo(subView);
}

void imageBackCallback(const sensor_msgs::ImageConstPtr& msg) {
  try {
    std::lock_guard<std::mutex> guard(back_mutex);
    frameback = cv_bridge::toCvShare(msg, "bgr8")->image.clone();
  } catch (...) {\
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

void imageFrontCallback(const sensor_msgs::ImageConstPtr& msg) {
  try {
   cv::Mat tmpfront = cv_bridge::toCvShare(msg, "bgr8")->image;
   if(tmpfront.total() ==0) {
     return;
   }
   cv::resize(tmpfront, tmpfront, cv::Size(SMALL_VIDEO_HIGHT, SMALL_VIDEO_WIDTH));
   std::lock_guard<std::mutex> guard(front_mutex);
   framefront = tmpfront.clone();
  } catch (...) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

void sendVideo()
{

  x264Encoder endocer(BIG_VIDEO_HIGHT, BIG_VIDEO_WIDTH, 3, 15);
  while (true)
  {
    
    cv::Mat tmpfront;
    cv::Mat tmpback ;
    {
      std::lock_guard<std::mutex> guard(front_mutex);
      tmpfront = framefront.clone();
    }
    {
        std::lock_guard<std::mutex> guard(back_mutex);
        tmpback = frameback.clone();
    }
  
    // if(tmpfront.rows ==0 || tmpfront.cols==0 || tmpfront.total()==0 || tmpback.total()==0||tmpback.rows ==0||tmpback.cols==0){
    //   continue;
    // }
    // cv::resize(tmpfront, tmpfront, cv::Size(SMALL_VIDEO_HIGHT, SMALL_VIDEO_WIDTH));
   
    try {
      if(tmpfront.rows > 0 && tmpfront.cols>0 ){  
        videoFuse(tmpback,tmpfront);

      }
      if( tmpback.rows > 0&&tmpback.cols>0){
        
        int size = endocer.EncodeOneFrame(tmpback);
        uchar* data = nullptr;
        int res = -1;
        data = endocer.GetEncodedFrame();
        // cv::imshow("view", tmpback);
        // cv::waitKey(10);
        res = Video_Port_Send_So((char*)data, size);
        std::cout<< "Send h264 with res " <<res<<"\r\n"; 
      }

      //LOG(INFO) << "Send h264 with res " << res;
      } catch (...) {
       // LOG(INFO) << "Error send h264";
      }
      
    
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "ai_video_send");
  ros::NodeHandle nh;
  while (Usb_Init_So() < 0) {        //图传模块初始化
    LOG(INFO) << "libar8020 usb init error, reinit in 5 secs";
    std::this_thread::sleep_for(std::chrono::seconds(5));
  }
  std::thread t(sendVideo);
	t.detach();	
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber subback =it.subscribe(TOPIC_CAMERA, 1, imageBackCallback);  //TOPIC_CAMERA_AI
  image_transport::Subscriber subfront =it.subscribe(TOPIC_CAMERA_TWO, 1, imageFrontCallback);  //TOPIC_CAMERA_AI
  ros::spin();
}
